#!/bin/bash

# Script to test ROS2 forward_simulator package build
set -e

echo "=== Testing ROS2 Forward Simulator Package Build ==="
echo "Current directory: $(pwd)"

# Check if we're in the right workspace
if [ ! -d "planning/forward_simulator" ]; then
    echo "Error: planning/forward_simulator directory not found. Please run from EPSILON workspace root."
    exit 1
fi

# Source ROS2 environment if available
if [ -f "/opt/ros/galactic/setup.bash" ]; then
    echo "Sourcing ROS2 Galactic..."
    source /opt/ros/galactic/setup.bash
elif [ -f "/opt/ros/galactic/setup.bash" ]; then
    echo "Sourcing ROS2 galactic..."
    source /opt/ros/galactic/setup.bash
elif [ -f "/opt/ros/foxy/setup.bash" ]; then
    echo "Sourcing ROS2 Foxy..."
    source /opt/ros/foxy/setup.bash
else
    echo "Warning: No ROS2 installation found. Continuing anyway..."
fi

# Test package structure
echo "=== Checking package structure ==="
required_files=("planning/forward_simulator/package.xml" "planning/forward_simulator/CMakeLists.txt" "planning/forward_simulator/include")
for file in "${required_files[@]}"; do
    if [ -e "$file" ]; then
        echo "✓ Found: $file"
    else
        echo "✗ Missing: $file"
        exit 1
    fi
done

# Check header files
echo "=== Checking header files ==="
header_files=(
    "planning/forward_simulator/include/forward_simulator/multimodal_forward.h"
    "planning/forward_simulator/include/forward_simulator/onlane_forward_simulation.h"
)
for file in "${header_files[@]}"; do
    if [ -e "$file" ]; then
        echo "✓ Found header: $file"
    else
        echo "✗ Missing header: $file"
        exit 1
    fi
done

# Check package.xml format
echo "=== Validating package.xml ==="
if xmllint --noout planning/forward_simulator/package.xml 2>/dev/null; then
    echo "✓ package.xml is valid XML"
else
    echo "✗ package.xml is invalid XML"
    exit 1
fi

# Test CMake syntax (basic check)
echo "=== Checking CMakeLists.txt syntax ==="
if grep -q "ament_package()" planning/forward_simulator/CMakeLists.txt; then
    echo "✓ CMakeLists.txt contains ament_package() - ROS2 format"
else
    echo "✗ CMakeLists.txt missing ament_package() - not ROS2 format"
    exit 1
fi

# Check for interface library
if grep -q "add_library.*INTERFACE" planning/forward_simulator/CMakeLists.txt; then
    echo "✓ CMakeLists.txt properly configured for header-only library"
else
    echo "✗ CMakeLists.txt not configured for header-only library"
    exit 1
fi

# Source install/setup.bash if it exists (for dependencies)
if [ -f "install/setup.bash" ]; then
    echo "Sourcing workspace setup..."
    source install/setup.bash
fi

# Try to build if colcon is available
if command -v colcon &> /dev/null; then
    echo "=== Attempting to build forward_simulator package ==="
    
    # Build dependencies first if needed
    echo "Building dependency packages..."
    colcon build --packages-select common vehicle_msgs vehicle_model --cmake-args -DBUILD_TESTING=OFF || echo "Some dependencies may not be available"
    
    # Source the newly built packages
    if [ -f "install/setup.bash" ]; then
        source install/setup.bash
    fi
    
    echo "Building forward_simulator..."
    # Try to build the forward_simulator package
    if colcon build --packages-select forward_simulator --cmake-args -DBUILD_TESTING=OFF; then
        echo "✓ forward_simulator builds successfully!"
        
        # Check if build artifacts exist
        if [ -d "build/forward_simulator" ] && [ -d "install/forward_simulator" ]; then
            echo "✓ Build artifacts created successfully"
            
            # Check installed headers
            if [ -d "install/forward_simulator/include/forward_simulator" ]; then
                echo "✓ Headers installed correctly"
                echo "Installed headers:"
                ls -la install/forward_simulator/include/forward_simulator/
            else
                echo "✗ Headers not installed properly"
                exit 1
            fi
        else
            echo "✗ Build artifacts not found"
            exit 1
        fi
    else
        echo "✗ Build failed. This might be due to missing dependencies."
        echo "The package structure is correct, but dependencies may be missing."
        exit 1
    fi
else
    echo "⚠ colcon not available, skipping build test"
fi

echo "=== ROS2 Forward Simulator Package Test Complete ==="
echo "✓ Package structure is correct"
echo "✓ Package follows ROS2 conventions"
echo "✓ Header-only library properly configured"
if command -v colcon &> /dev/null; then
    echo "✓ Package builds successfully"
fi
echo ""
echo "The 'forward_simulator' directory has been successfully converted to a ROS2 header-only package!"